﻿/*------------------------------------------------------------------------------------------*\
This file contains material supporting chapter 10 of the book:
OpenCV3 Computer Vision Application Programming Cookbook
Third Edition
by Robert Laganiere, Packt Publishing, 2016.

This program is free software; permission is hereby granted to use, copy, modify,
and distribute this source code, or portions thereof, for any purpose, without fee,
subject to the restriction that the copyright notice may not be removed
or altered from any source or altered source distribution.
The software is released on an as-is basis and without any warranties of any kind.
In particular, the software is not guaranteed to be fault-tolerant or free from failure.
The author disclaims all warranties with regard to this software, any use,
and any consequent failure, is purely the responsibility of the user.

Copyright (C) 2016 Robert Laganiere, www.laganiere.name
\*------------------------------------------------------------------------------------------*/

#if !defined MATCHER
#define MATCHER
#include <QWidget>
#include <iostream>
#include <vector>
#include <opencv2/core.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/features2d.hpp>
#include <opencv2/calib3d.hpp>
#include <opencv2/xfeatures2d.hpp>

#define NOCHECK      0
#define CROSSCHECK   1
#define RATIOCHECK   2
#define BOTHCHECK    3

class RobustMatcher {

  private:

      cv::Ptr<cv::FeatureDetector> detector;
	  cv::Ptr<cv::DescriptorExtractor> descriptor;
	  int normType;
      float ratio;
      bool refineF;
      bool refineM;
      double distance;
      double confidence;

  public:
      QString info;

	  RobustMatcher(const cv::Ptr<cv::FeatureDetector> &detector, 
		            const cv::Ptr<cv::DescriptorExtractor> &descriptor= cv::Ptr<cv::DescriptorExtractor>())
		  : detector(detector), descriptor(descriptor),normType(cv::NORM_L2), 
		    ratio(0.8f), refineF(true), refineM(true), confidence(0.98), distance(1.0) {	  

		if (!this->descriptor) { 
			this->descriptor = this->detector;
        }
        info="";
	  }

	  void setFeatureDetector(const cv::Ptr<cv::FeatureDetector>& detect) {

		  this->detector= detect;
	  }

	  void setDescriptorExtractor(const cv::Ptr<cv::DescriptorExtractor>& desc) {

		  this->descriptor= desc;
	  }

	  void setNormType(int norm) {

		  normType= norm;
	  }

	  void setMinDistanceToEpipolar(double d) {

		  distance= d;
	  }

	  void setConfidenceLevel(double c) {

		  confidence= c;
	  }

	  void setRatio(float r) {

		  ratio= r;
	  }

	  void refineFundamental(bool flag) {

		  refineF= flag;
      }

	  void refineMatches(bool flag) {

		  refineM= flag;
	  }

      int ratioTest(const std::vector<std::vector<cv::DMatch> >& inputMatches,
		            std::vector<cv::DMatch>& outputMatches) {

		int removed=0;

        for (std::vector<std::vector<cv::DMatch> >::const_iterator matchIterator= inputMatches.begin();
			 matchIterator!= inputMatches.end(); ++matchIterator) {

                 if ((matchIterator->size() > 1) &&
					 (*matchIterator)[0].distance/(*matchIterator)[1].distance < ratio) {

					 outputMatches.push_back((*matchIterator)[0]);

				 } else {

					 removed++;
				 }
		}

		return removed;
	  }

	  void symmetryTest(const std::vector<cv::DMatch>& matches1,
		                const std::vector<cv::DMatch>& matches2,
					    std::vector<cv::DMatch>& symMatches) {

		for (std::vector<cv::DMatch>::const_iterator matchIterator1= matches1.begin();
			 matchIterator1!= matches1.end(); ++matchIterator1) {

			for (std::vector<cv::DMatch>::const_iterator matchIterator2= matches2.begin();
				matchIterator2!= matches2.end(); ++matchIterator2) {

				if (matchIterator1->queryIdx == matchIterator2->trainIdx  && 
					matchIterator2->queryIdx == matchIterator1->trainIdx) {

                        symMatches.push_back(*matchIterator1);
				}
			}
		}
	  }

      void ratioAndSymmetryTest(const std::vector<std::vector<cv::DMatch> >& matches1,
                                const std::vector<std::vector<cv::DMatch> >& matches2,
					            std::vector<cv::DMatch>& outputMatches) {

		std::vector<cv::DMatch> ratioMatches1;
        int removed= ratioTest(matches1,ratioMatches1);
        info=info+"Number of matched points 1->2 (ratio test) : "+QString::number(ratioMatches1.size())+"\n";

		std::vector<cv::DMatch> ratioMatches2;
        removed= ratioTest(matches2,ratioMatches2);
        info=info+"Number of matched points 1->2 (ratio test) : "+QString::number(ratioMatches2.size())+"\n";

		symmetryTest(ratioMatches1,ratioMatches2,outputMatches);
        info=info+"Number of matched points (symmetry test): "+QString::number(outputMatches.size())+"\n";
	  }

	  cv::Mat ransacTest(const std::vector<cv::DMatch>& matches,
		                 std::vector<cv::KeyPoint>& keypoints1, 
						 std::vector<cv::KeyPoint>& keypoints2,
					     std::vector<cv::DMatch>& outMatches) {

		std::vector<cv::Point2f> points1, points2;	

		for (std::vector<cv::DMatch>::const_iterator it= matches.begin();
			 it!= matches.end(); ++it) {

             points1.push_back(keypoints1[it->queryIdx].pt);
			 points2.push_back(keypoints2[it->trainIdx].pt);
	    }

		std::vector<uchar> inliers(points1.size(),0);
		cv::Mat fundamental= cv::findFundamentalMat(
            points1,points2,
            inliers,
            cv::FM_RANSAC,
            distance,
            confidence);
	

		std::vector<uchar>::const_iterator itIn= inliers.begin();
		std::vector<cv::DMatch>::const_iterator itM= matches.begin();

		for ( ;itIn!= inliers.end(); ++itIn, ++itM) {

            if (*itIn) {

				outMatches.push_back(*itM);
			}
		}

        if (refineF || refineM) {

			points1.clear();
			points2.clear();
	
			for (std::vector<cv::DMatch>::const_iterator it= outMatches.begin();
				 it!= outMatches.end(); ++it) {

                 points1.push_back(keypoints1[it->queryIdx].pt);
				 points2.push_back(keypoints2[it->trainIdx].pt);
			}

			fundamental= cv::findFundamentalMat(
                points1,points2,
                cv::FM_8POINT);

			if (refineM) {

                std::vector<cv::Point2f> newPoints1, newPoints2;
                correctMatches(fundamental,
                               points1, points2,
                               newPoints1, newPoints2);
				for (int i=0; i< points1.size(); i++) {

                    info=info+"("+QString::number(keypoints1[outMatches[i].queryIdx].pt.x)+","+QString::number(keypoints1[outMatches[i].queryIdx].pt.y)+") -> ";
                    info=info+"("+QString::number(newPoints1[i].x)+","+QString::number(newPoints1[i].y)+")\n";
                    info=info+"("+QString::number(keypoints2[outMatches[i].trainIdx].pt.x)+","+QString::number(keypoints2[outMatches[i].trainIdx].pt.y) +") -> ";
                    info=info+"("+QString::number(newPoints1[i].x)+","+QString::number(newPoints1[i].y)+")\n";

					keypoints1[outMatches[i].queryIdx].pt.x= newPoints1[i].x;
					keypoints1[outMatches[i].queryIdx].pt.y= newPoints1[i].y;
					keypoints2[outMatches[i].trainIdx].pt.x= newPoints2[i].x;
					keypoints2[outMatches[i].trainIdx].pt.y= newPoints2[i].y;
				}
			}
		}


		return fundamental;
      }

      cv::Mat match(cv::Mat& image1, cv::Mat& image2,
          std::vector<cv::DMatch>& matches,
		  std::vector<cv::KeyPoint>& keypoints1, std::vector<cv::KeyPoint>& keypoints2,
          int check=CROSSCHECK) {

		detector->detect(image1,keypoints1);
		detector->detect(image2,keypoints2);

        info=info+"Number of feature points (1): "+ QString::number(keypoints1.size())+"\n";

        info=info+"Number of feature points (2): "+ QString::number(keypoints2.size())+"\n";

		cv::Mat descriptors1, descriptors2;
		descriptor->compute(image1,keypoints1,descriptors1);
		descriptor->compute(image2,keypoints2,descriptors2);

        info=info+"descriptor matrix size: "+ QString::number(descriptors1.rows)+"by"+QString::number(descriptors1.cols)+"\n";

        cv::BFMatcher matcher(normType,
                              check==CROSSCHECK);

        std::vector<std::vector<cv::DMatch> > matches1;
        std::vector<std::vector<cv::DMatch> > matches2;
	    std::vector<cv::DMatch> outputMatches;

        if (check==RATIOCHECK || check==BOTHCHECK) {
			matcher.knnMatch(descriptors1,descriptors2, 
                matches1,
                2);

            info=info+"Number of matched points 1->2: "+ QString::number(matches1.size())+"\n";

            if (check==BOTHCHECK) {
				matcher.knnMatch(descriptors2,descriptors1, 
                    matches2,
                    2);

                info=info+"Number of matched points 2->1: "+ QString::number(matches2.size())+"\n";
			}

		} 

		switch (check) {

			case CROSSCHECK:
                matcher.match(descriptors1,descriptors2,outputMatches);
                info=info+"Number of matched points 1->2 (after cross-check): "+ QString::number(outputMatches.size())+"\n";
				break;
			case RATIOCHECK:
                ratioTest(matches1,outputMatches);
                info=info+"Number of matched points 1->2 (after ratio test): "+ QString::number(outputMatches.size())+"\n";
				break;
			case BOTHCHECK:
                ratioAndSymmetryTest(matches1,matches2,outputMatches);
                info=info+"Number of matched points 1->2 (after ratio and cross-check): "+ QString::number(outputMatches.size())+"\n";
				break;
			case NOCHECK:
			default:
                matcher.match(descriptors1,descriptors2,outputMatches);
                info=info+"Number of matched points 1->2: "+ QString::number(outputMatches.size())+"\n";
				break;
		}

        cv::Mat fundamental= ransacTest(outputMatches, keypoints1, keypoints2, matches);
        info=info+"Number of matched points (after RANSAC): "+ QString::number(matches.size())+"\n";
		return fundamental;
	}

      cv::Mat matchBook(cv::Mat& image1, cv::Mat& image2,
          std::vector<cv::DMatch>& matches,
		  std::vector<cv::KeyPoint>& keypoints1, std::vector<cv::KeyPoint>& keypoints2) { 

		detector->detect(image1,keypoints1);
		detector->detect(image2,keypoints2);

		cv::Mat descriptors1, descriptors2;
		descriptor->compute(image1,keypoints1,descriptors1);
		descriptor->compute(image2,keypoints2,descriptors2);

        cv::BFMatcher matcher(normType,
                              true);

	    std::vector<cv::DMatch> outputMatches;
		matcher.match(descriptors1,descriptors2,outputMatches);

		cv::Mat fundamental= ransacTest(outputMatches, keypoints1, keypoints2, matches);

		return fundamental;
	}

};

#endif
